#include <stdio.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <qmc5883l.h>
#include <string.h>
#include "esp_check.h"
#include <math.h>

#define TAG "exmaple_qmc5883_task"

void exmaple_qmc5883_task(){
    qmc5883l_t dev;

    memset(&dev, 0, sizeof(qmc5883l_t));
    ESP_ERROR_CHECK(i2cdev_init());
    ESP_ERROR_CHECK(qmc5883l_init_desc(&dev, 0x2c, 0, GPIO_NUM_1, GPIO_NUM_2));

    // 50Hz data rate, 128 samples, -2G..+2G range
    ESP_ERROR_CHECK(qmc5883l_set_config(&dev, QMC5883L_DR_50, QMC5883L_OSR1_8, QMC5883L_RNG_30));

    uint8_t chip_id = 0;
    qmc5883l_get_chip_id(&dev,&chip_id);
    printf("chip_id=%X\r\n",chip_id);

    while (1)
    {
        qmc5883l_data_t data;
        if (qmc5883l_get_data(&dev, &data) == ESP_OK){
            /* float is used in printf(). you need non-default configuration in
             * sdkconfig for ESP8266, which is enabled by default for this
             * example. see sdkconfig.defaults.esp8266
             */
            //printf("Magnetic data: X:%.2f mG, Y:%.2f mG, Z:%.2f mG\n", data.x, data.y, data.z);

            // 计算水平航向角（忽略Z轴）
            //float M_PI = 3.1415926;
            //float heading = atan2f(data.y, data.x) * 180.0 / M_PI;
            //if (heading < 0) heading += 360.0;

            // 计算三维航向角（更精确）
            //float heading_3d = atan2f(data.y, sqrtf(data.x*data.x + data.z*data.z)) * 180.0 / M_PI;

            //printf("Magnetic data: X:%.2f mG, Y:%.2f mG, Z:%.2f mG  head:%.2f\n", data.x, data.y, data.z,heading_3d);
            float heading1 = atan2f(data.y, data.x) * 180.0 / M_PI;  // 正确
            float heading2 = atan2f(data.x, data.y) * 180.0 / M_PI;  // 错误
            
            if (heading1 < 0) heading1 += 360.0;
            if (heading2 < 0) heading2 += 360.0;
            // 计算水平强度
            float horizontal_strength = sqrtf(data.x * data.x + data.y * data.y);
            float total_strength = sqrtf(data.x * data.x + data.y * data.y + data.z * data.z);
            float inclination = atan2f(data.z, horizontal_strength) * 180.0 / M_PI;
            
            ESP_LOGI(TAG, "=== 磁场分析 ===");
            ESP_LOGI(TAG, "各轴分量: X=%.1f, Y=%.1f, Z=%.1f mG", data.x, data.y, data.z);
            ESP_LOGI(TAG, "水平强度: %.1f mG", horizontal_strength);
            ESP_LOGI(TAG, "总强度: %.1f mG", total_strength);
            ESP_LOGI(TAG, "磁倾角: %.1f°", inclination);
            ESP_LOGI(TAG, "当前航向: %.1f°  %.1f°   ", heading1,heading2);
                
        }else
            printf("Could not read QMC5883L data\n");

        vTaskDelay(pdMS_TO_TICKS(250));
    }
}

void exmaple_qmc5883_test(){
    xTaskCreatePinnedToCore(exmaple_qmc5883_task, "qmc5883l_test", configMINIMAL_STACK_SIZE * 4, NULL, 5, NULL, APP_CPU_NUM);
}